﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Forms;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningAlgorithms;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using System.Diagnostics;

namespace MotionPlanning.MotionPlanningComponents.Visualizations
{
    //
    // Visualizer for System_OneLink
    //
    public class Visualizer_TwoLink : Visualizer
    {
        Graphics instantViewHolder;
        Bitmap instantViewSheet;
        Graphics instantView;
        Rectangle instViewRect;
        Graphics canvas;
        Rectangle canvasRect;
        SolidBrush brushCanvasClear;
        SolidBrush brushNodePlot;
        SolidBrush brushStateRed;
        Pen edgeBlue;
        Pen edgeRed;
        Pen edgeBlueVirtual;
        Pen canvasAxisGray;
        RichTextBox textOutput;

        private int screen_width;
        private int screen_height;
        private int center_x;
        private int center_y;
        private double calibrater_x;
        private double calibrater_y;
        private Planner_RRTBased planner_subject;
        private float[,] stage;
        private int n_x;
        private int n_y;

        public Visualizer_TwoLink(Panel CanvasRegion)
            :base(CanvasRegion)
        {
            canvas = CanvasRegion.CreateGraphics();
            canvasRect = new Rectangle(0, 0, CanvasRegion.Width, CanvasRegion.Height);
            brushCanvasClear = new SolidBrush(Color.FromArgb(255, 255, 255));
            brushNodePlot = new SolidBrush(Color.Red);
            canvasAxisGray = new Pen(Color.Gray);
            edgeBlue = new Pen(Color.Blue, 4);
            edgeRed = new Pen(Color.Red, 4);
            edgeBlueVirtual = new Pen(Color.Red, 2);
            brushStateRed = new SolidBrush(Color.Red);
            calibrater_x = canvasRect.Width / 2;
            calibrater_y = -canvasRect.Height / 2;
            center_x = CanvasRegion.Width / 2;
            center_y = CanvasRegion.Height / 2;
            screen_width = CanvasRegion.Width;
            screen_height = CanvasRegion.Height;
            n_x = 500;
            n_y = 500;
            stage = new float[n_x, n_y];
            for (int i = 0; i < n_x; ++i)
                for (int j = 0; j < n_y; ++j)
                    stage[i, j] = float.MaxValue;
        }

        public Visualizer_TwoLink(Panel CanvasRegion, Panel instantView, RichTextBox output)
            :base(CanvasRegion)
        {
            textOutput = output;
            this.canvas = CanvasRegion.CreateGraphics();
            canvasRect = new Rectangle(0, 0, CanvasRegion.Width, CanvasRegion.Height);
            this.instantView = instantView.CreateGraphics();
            instantViewHolder = instantView.CreateGraphics();
            instViewRect = new Rectangle(0, 0, instantView.Width, instantView.Height);
            brushCanvasClear = new SolidBrush(Color.FromArgb(255, 255, 255));
            brushNodePlot = new SolidBrush(Color.Red);
            canvasAxisGray = new Pen(Color.Gray);
            edgeBlue = new Pen(Color.Blue, 4);
            edgeRed = new Pen(Color.Red, 4);
            edgeBlueVirtual = new Pen(Color.Red, 2);
            brushStateRed = new SolidBrush(Color.Red);
            calibrater_x = canvasRect.Width / 2;
            calibrater_y = -canvasRect.Height / 2;
            center_x = CanvasRegion.Width / 2;
            center_y = CanvasRegion.Height / 2;
            screen_width = CanvasRegion.Width;
            screen_height = CanvasRegion.Height;

            instantViewSheet = new Bitmap(instViewRect.Width, instViewRect.Height);
            this.instantView = Graphics.FromImage(instantViewSheet);
        }

        public void VisualizeState(double[] q_vis)
        {
            double angle1 = q_vis[0];
            double angle2 = q_vis[1];
            float L1 = 1.5f;
            float L2 = 1.5f;
            double end1_x = L1 * Math.Cos(q_vis[0]);
            double end1_y = L1 * Math.Sin(q_vis[0]);
            canvas.DrawLine(edgeBlue,
                center_x,
                center_y,
                (float)(center_x + end1_x * calibrater_x),
                (float)(center_y + end1_y * calibrater_y));
            double end2_x = L2 * Math.Cos(q_vis[1]);
            double end2_y = L2 * Math.Sin(q_vis[1]);
            canvas.DrawLine(edgeRed,
                (float)(center_x + end1_x * calibrater_x),
                (float)(center_y + end1_y * calibrater_y),
                (float)(center_x + end1_x * calibrater_x + end2_x * calibrater_x),
                (float)(center_y + end1_y * calibrater_y + end2_y * calibrater_y));
        }

        public void ClearCanvas()
        {
            canvas.FillRectangle(brushCanvasClear, canvasRect);
            canvas.DrawLine(canvasAxisGray, 0, center_y, screen_width, center_y);
            canvas.DrawLine(canvasAxisGray, center_x, 0, center_x, screen_height);
        }

        public void PlotVerticesWithCost(List<object> train)
        {
            throw new NotImplementedException();
        }

        public void PlotEdges(List<object> train)
        {
            double distance_threshold2 = 200 * 200;
            double[] q1p = new double[2];
            double[] q2p = new double[2];
            for (int i = 0; i < train.Count; ++i)
            {
                List<Edge> pe = ((Node)train[i]).get_Parents();
                if (pe == null) continue;

                for (int j = pe.Count - 1; j < pe.Count; ++j)
                {
                    Pen edgePen = edgeBlueVirtual;
                    if (j == 0)
                    {
                        edgePen = edgeBlue;
                        //continue;  //comment this line to show RRT edges
                        //uncomment this line to show RDG edges without RRT edges
                    }
                    Edge p = pe[j];
                    double[] q1 = p.get_entry().get_Value().get_q();
                    double[] q2 = p.get_exit().get_Value().get_q();

                    int screen_x1 = (int)(q1[0] * this.calibrater_x + center_x);
                    int screen_y1 = (int)(q1[1] * this.calibrater_y + center_y);

                    int screen_x2 = (int)(q2[0] * this.calibrater_x + center_x);
                    int screen_y2 = (int)(q2[1] * this.calibrater_y + center_y);

                    double dx = screen_x1 - screen_x2;
                    double dy = screen_y1 - screen_y2;
                    double distance2 = dx * dx + dy * dy;
                    if (distance2 > distance_threshold2 && q2[0] * q1[0] < 0)
                    {
                        q2p[0] = q2[0];
                        q2p[1] = q2[1];
                        if (q2p[0] < 0)
                            q2p[0] += Math.PI + Math.PI;
                        else
                            q2p[0] -= Math.PI + Math.PI;
                        int screen_x2p = (int)(q2p[0] * this.calibrater_x + center_x);
                        int screen_y2p = (int)(q2p[1] * this.calibrater_y + center_y);
                        canvas.DrawLine(edgePen, screen_x1, screen_y1, screen_x2p, screen_y2p);

                        q1p[0] = q1[0];
                        q1p[1] = q1[1];
                        if (q1p[0] < 0)
                            q1p[0] += Math.PI + Math.PI;
                        else
                            q1p[0] -= Math.PI + Math.PI;
                        int screen_x1p = (int)(q1p[0] * this.calibrater_x + center_x);
                        int screen_y1p = (int)(q1p[1] * this.calibrater_y + center_y);
                        canvas.DrawLine(edgePen, screen_x1p, screen_y1p, screen_x2, screen_y2);
                    }
                    else
                        canvas.DrawLine(edgePen, screen_x1, screen_y1, screen_x2, screen_y2);
                }
            }
        }

        public void PlotVertices(List<object> train)
        {
            float pointRadius = 1f;
            for (int i = 0; i < train.Count; ++i)
            {
                double[] cq = ((Node)train[i]).get_Value().get_q();
                int screen_x = (int)(cq[0] * this.calibrater_x + center_x);
                int screen_y = (int)(cq[1] * this.calibrater_y + center_y);
                canvas.FillEllipse(brushStateRed, screen_x - pointRadius, screen_y - pointRadius, pointRadius + pointRadius, pointRadius + pointRadius);
            }
        }

        public void PlotTrajectory(Trajectory trajectory)
        {
            List<Node> nodeArray = trajectory.GetNodes();
            List<object> train = new List<object>(nodeArray.Count);
            for (int i = 0; i < nodeArray.Count; ++i)
            {
                train.Add(nodeArray[i]);
            }

            PlotEdges(train);
        }

        public void SetHostPlanner(Planner_RRTBased host)
        {
            planner_subject = host;
            double[] q_max = planner_subject.GetSystem().Get_q_max();
            double[] q_min = planner_subject.GetSystem().Get_q_min();
            double range_x = (q_max[0] - q_min[0]) / 2;
            double range_y = (q_max[1] - q_min[1]) / 2;
            this.calibrater_x = canvasRect.Width / 2 / range_x;
            this.calibrater_y = -canvasRect.Height / 2 / range_y;
        }
    }
}
